import math

def theta_to_Rxyz(theta: float):
    """
    前提：工具头垂直向下（tool Z = -world Z），theta 为绕世界 Z 轴的旋转角（弧度）。
    返回：UR TCP 姿态中的旋转向量 (rx, ry, rz)。
    """
    # 轴：u = [cos(theta/2), sin(theta/2), 0]，角度恒为 π
    half = 0.5 * theta
    rx = math.pi * math.cos(half)
    ry = math.pi * math.sin(half)
    rz = 0.0
    return rx, ry, rz


def Rxyz_to_theta(rx: float, ry: float, rz: float):
    """
    前提：姿态属于“工具头垂直向下 + 绕世界 Z 轴”的这一族（即 |r|≈π 且 rz≈0）。
    返回：对应的 theta（弧度）。
    """
    # 长度应当 ≈ π，数值上允许少许偏差
    r = math.sqrt(rx*rx + ry*ry + rz*rz)
    # 可做个小保护：若与 π 偏差很大，说明不在该姿态族，按需求处理
    # if abs(r - math.pi) > 1e-3 or abs(rz) > 1e-6: raise ValueError("Not in the vertical-down family")

    # 由 rx = π cos(theta/2), ry = π sin(theta/2)
    half = math.atan2(ry, rx)            # 得到 theta/2 ∈ (-π, π]
    theta = 2.0 * half                   # theta ∈ (-2π, 2π]
    # 如需连续角度，可在外部做 unwrap
    return theta
